#ifndef URCONTROLPLANE_HPP
#define URCONTROLPLANE_HPP

#include "urcontrola.hpp"
#include "imagetoplane.hpp"

class URControlPlane : public URControlA
{
public:
    ImageToPlane i2p;

    URControlPlane(std::string ipPC="192.168.1.14", std::string ipUR="192.168.1.13", unsigned short portPC=1235, unsigned short portUR=30003, unsigned short modbusPort=502)
        : URControlA(ipPC, ipUR, portPC, portUR, modbusPort)
    {
        i2p.test();
    }

    void test()
    {
        cv::Mat chessboard;
        i2p.Load(chessboard, "chess.yml");

        for(int i=0;i<chessboard.total();i++)
        {

        cv::Vec3d cp=chessboard.at<cv::Vec3d>(i);

        cp[2]=CV_PI/5*i;

        cv::Vec6d pose=i2p.PixelVectorToHeadPos(cv::Vec3d(0,0,0.21), cp);

        std::cout<<pose<<"\n"<<std::flush;

        SetPose(pose[0], pose[1],pose[2]+0.01,pose[3],pose[4],pose[5],0.1);

        std::this_thread::sleep_for(std::chrono::seconds(1));
        }
    }
};

#endif // URCONTROLPLANE_HPP

